<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>Cheetah Software: spatial.h Source File</title>
<link href="../../tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../jquery.js"></script>
<script type="text/javascript" src="../../dynsections.js"></script>
<link href="../../navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../resize.js"></script>
<script type="text/javascript" src="../../navtreedata.js"></script>
<script type="text/javascript" src="../../navtree.js"></script>
<script type="text/javascript">
  $(document).ready(initResizable);
  $(window).load(resizeHeight);
</script>
<link href="../../search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../search/searchdata.js"></script>
<script type="text/javascript" src="../../search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { init_search(); });
</script>
<link href="../../doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">Cheetah Software
   &#160;<span id="projectnumber">1.0</span>
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "../../search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="../../index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="../../namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="../../annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="../../files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="../../search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="../../search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="../../files.html"><span>File&#160;List</span></a></li>
      <li><a href="../../globals.html"><span>File&#160;Members</span></a></li>
    </ul>
  </div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
$(document).ready(function(){initNavTree('d9/dc8/spatial_8h_source.html','../../');});
</script>
<div id="doc-content">
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">spatial.h</div>  </div>
</div><!--header-->
<div class="contents">
<a href="../../d9/dc8/spatial_8h.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;</div><div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="preprocessor">#ifndef LIBBIOMIMETICS_SPATIAL_H</span></div><div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="preprocessor">#define LIBBIOMIMETICS_SPATIAL_H</span></div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;</div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../d4/dfb/orientation__tools_8h.html">Math/orientation_tools.h</a>&quot;</span></div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;</div><div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;<span class="preprocessor">#include &lt;eigen3/Eigen/Dense&gt;</span></div><div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;</div><div class="line"><a name="l00015"></a><span class="lineno">   15</span>&#160;<span class="preprocessor">#include &lt;cmath&gt;</span></div><div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;<span class="preprocessor">#include &lt;iostream&gt;</span></div><div class="line"><a name="l00017"></a><span class="lineno">   17</span>&#160;<span class="preprocessor">#include &lt;type_traits&gt;</span></div><div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;</div><div class="line"><a name="l00019"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html">   19</a></span>&#160;<span class="keyword">namespace </span><a class="code" href="../../de/d1a/namespacespatial.html">spatial</a> {</div><div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160;<span class="keyword">using namespace </span><a class="code" href="../../da/d9b/namespaceori.html">ori</a>;</div><div class="line"><a name="l00021"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51">   21</a></span>&#160;<span class="keyword">enum class</span> <a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51">JointType</a> { <a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51a35fa8acd1b8b1beb23e6c2e24313570c">Prismatic</a>, <a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51a358561ac4d41f41d479b7c54b19b639e">Revolute</a>, <a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51a70739581ffec4f3db3f3fe5c0a309c3a">FloatingBase</a>, <a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51af80a4ad87fee7c9fdc19b7769495fdb5">Nothing</a> };</div><div class="line"><a name="l00022"></a><span class="lineno">   22</span>&#160;</div><div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00028"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#acab0e3d82f9a5385d482b83f2ee88eb1">   28</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a217c4c7a22cd434b9700b183b07f1505">SXform&lt;T&gt;</a> <a class="code" href="../../de/d1a/namespacespatial.html#acab0e3d82f9a5385d482b83f2ee88eb1">spatialRotation</a>(<a class="code" href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5">CoordinateAxis</a> axis, T theta) {</div><div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;  static_assert(std::is_floating_point&lt;T&gt;::value,</div><div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;                <span class="stringliteral">&quot;must use floating point value&quot;</span>);</div><div class="line"><a name="l00031"></a><span class="lineno">   31</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;T&gt;</a> R = <a class="code" href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">coordinateRotation</a>(axis, theta);</div><div class="line"><a name="l00032"></a><span class="lineno">   32</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a217c4c7a22cd434b9700b183b07f1505">SXform&lt;T&gt;</a> <a class="code" href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383">X</a> = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a217c4c7a22cd434b9700b183b07f1505">SXform&lt;T&gt;::Zero</a>();</div><div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;  X.template topLeftCorner&lt;3, 3&gt;() = R;</div><div class="line"><a name="l00034"></a><span class="lineno">   34</span>&#160;  X.template bottomRightCorner&lt;3, 3&gt;() = R;</div><div class="line"><a name="l00035"></a><span class="lineno">   35</span>&#160;  <span class="keywordflow">return</span> X;</div><div class="line"><a name="l00036"></a><span class="lineno">   36</span>&#160;}</div><div class="line"><a name="l00037"></a><span class="lineno">   37</span>&#160;</div><div class="line"><a name="l00042"></a><span class="lineno">   42</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00043"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#acd5568b97b640537ab22aaf458c73980">   43</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#acd5568b97b640537ab22aaf458c73980">motionCrossMatrix</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; v) {</div><div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;  static_assert(T::ColsAtCompileTime == 1 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160;                <span class="stringliteral">&quot;Must have 6x1 vector&quot;</span>);</div><div class="line"><a name="l00046"></a><span class="lineno">   46</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;typename T::Scalar&gt;</a> m;</div><div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;  m &lt;&lt; 0, -v(2), v(1), 0, 0, 0, v(2), 0, -v(0), 0, 0, 0, -v(1), v(0), 0, 0, 0,</div><div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;      0,</div><div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;</div><div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;      0, -v(5), v(4), 0, -v(2), v(1), v(5), 0, -v(3), v(2), 0, -v(0), -v(4),</div><div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;      v(3), 0, -v(1), v(0), 0;</div><div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;  <span class="keywordflow">return</span> m;</div><div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;}</div><div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;</div><div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00060"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#a261bcef5a53b2fb64a5c7370420a1275">   60</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#a261bcef5a53b2fb64a5c7370420a1275">forceCrossMatrix</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; v) {</div><div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;typename T::Scalar&gt;</a> <a class="code" href="../../dc/dd7/test__casadi_8cpp.html#ae20d7fba59865a21504b65cdb44cbfe5">f</a>;</div><div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;  f &lt;&lt; 0, -v(2), v(1), 0, -v(5), v(4), v(2), 0, -v(0), v(5), 0, -v(3), -v(1),</div><div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;      v(0), 0, -v(4), v(3), 0, 0, 0, 0, 0, -v(2), v(1), 0, 0, 0, v(2), 0, -v(0),</div><div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;      0, 0, 0, -v(1), v(0), 0;</div><div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;  <span class="keywordflow">return</span> <a class="code" href="../../dc/dd7/test__casadi_8cpp.html#ae20d7fba59865a21504b65cdb44cbfe5">f</a>;</div><div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;}</div><div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;</div><div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00073"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#a49f73699d3c57d07835e8ab61af3f34a">   73</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#a49f73699d3c57d07835e8ab61af3f34a">motionCrossProduct</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; a,</div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;                        <span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; b) {</div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;  static_assert(T::ColsAtCompileTime == 1 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;                <span class="stringliteral">&quot;Must have 6x1 vector&quot;</span>);</div><div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;typename T::Scalar&gt;</a> mv;</div><div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;  mv &lt;&lt; a(1) * b(2) - a(2) * b(1), a(2) * b(0) - a(0) * b(2),</div><div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;      a(0) * b(1) - a(1) * b(0),</div><div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;      a(1) * b(5) - a(2) * b(4) + a(4) * b(2) - a(5) * b(1),</div><div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;      a(2) * b(3) - a(0) * b(5) - a(3) * b(2) + a(5) * b(0),</div><div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;      a(0) * b(4) - a(1) * b(3) + a(3) * b(1) - a(4) * b(0);</div><div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;  <span class="keywordflow">return</span> mv;</div><div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;}</div><div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;</div><div class="line"><a name="l00090"></a><span class="lineno">   90</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00091"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">   91</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">forceCrossProduct</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; a,</div><div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;                       <span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; b) {</div><div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;  static_assert(T::ColsAtCompileTime == 1 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;                <span class="stringliteral">&quot;Must have 6x1 vector&quot;</span>);</div><div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;typename T::Scalar&gt;</a> mv;</div><div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;  mv &lt;&lt; b(2) * a(1) - b(1) * a(2) - b(4) * a(5) + b(5) * a(4),</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;      b(0) * a(2) - b(2) * a(0) + b(3) * a(5) - b(5) * a(3),</div><div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;      b(1) * a(0) - b(0) * a(1) - b(3) * a(4) + b(4) * a(3),</div><div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;      b(5) * a(1) - b(4) * a(2), b(3) * a(2) - b(5) * a(0),</div><div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;      b(4) * a(0) - b(3) * a(1);</div><div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;  <span class="keywordflow">return</span> mv;</div><div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;}</div><div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;</div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00108"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#a30b33586f68f1f6b31718848914a90ca">  108</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#a30b33586f68f1f6b31718848914a90ca">sxformToHomogeneous</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; X) {</div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;  static_assert(T::ColsAtCompileTime == 6 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;                <span class="stringliteral">&quot;Must have 6x6 matrix&quot;</span>);</div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a5e48cf69558cf96d92f71803cf9d19a4">Mat4&lt;typename T::Scalar&gt;</a> H = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a5e48cf69558cf96d92f71803cf9d19a4">Mat4&lt;typename T::Scalar&gt;::Zero</a>();</div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;typename T::Scalar&gt;</a> R = X.template topLeftCorner&lt;3, 3&gt;();</div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;typename T::Scalar&gt;</a> skewR = X.template bottomLeftCorner&lt;3, 3&gt;();</div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;  H.template topLeftCorner&lt;3, 3&gt;() = R;</div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;  H.template topRightCorner&lt;3, 1&gt;() = <a class="code" href="../../da/d9b/namespaceori.html#a0d9277fde185aecf3eae4a6d6450d25f">matToSkewVec</a>(skewR * R.transpose());</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;  H(3, 3) = 1;</div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;  <span class="keywordflow">return</span> H;</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;}</div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00124"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#a39cbc59fb7656b397d13c5a52799bf22">  124</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#a39cbc59fb7656b397d13c5a52799bf22">homogeneousToSXform</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; H) {</div><div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;  static_assert(T::ColsAtCompileTime == 4 &amp;&amp; T::RowsAtCompileTime == 4,</div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;                <span class="stringliteral">&quot;Must have 4x4 matrix&quot;</span>);</div><div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;typename T::Scalar&gt;</a> R = H.template topLeftCorner&lt;3, 3&gt;();</div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> translate = H.template topRightCorner&lt;3, 1&gt;();</div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;typename T::Scalar&gt;</a> <a class="code" href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383">X</a> = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;typename T::Scalar&gt;::Zero</a>();</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;  X.template topLeftCorner&lt;3, 3&gt;() = R;</div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;  X.template bottomLeftCorner&lt;3, 3&gt;() = <a class="code" href="../../da/d9b/namespaceori.html#a153f2a76610f0c5524da3f8bf8fa4dfa">vectorToSkewMat</a>(translate) * R;</div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;  X.template bottomRightCorner&lt;3, 3&gt;() = R;</div><div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;  <span class="keywordflow">return</span> X;</div><div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;}</div><div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;</div><div class="line"><a name="l00139"></a><span class="lineno">  139</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T, <span class="keyword">typename</span> T2&gt;</div><div class="line"><a name="l00140"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">  140</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">createSXform</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; R,</div><div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;                  <span class="keyword">const</span> Eigen::MatrixBase&lt;T2&gt;&amp; r) {</div><div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;  static_assert(T::ColsAtCompileTime == 3 &amp;&amp; T::RowsAtCompileTime == 3,</div><div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;                <span class="stringliteral">&quot;Must have 3x3 matrix&quot;</span>);</div><div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;  static_assert(T2::ColsAtCompileTime == 1 &amp;&amp; T2::RowsAtCompileTime == 3,</div><div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;                <span class="stringliteral">&quot;Must have 3x1 matrix&quot;</span>);</div><div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;typename T::Scalar&gt;</a> <a class="code" href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383">X</a> = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;typename T::Scalar&gt;::Zero</a>();</div><div class="line"><a name="l00147"></a><span class="lineno">  147</span>&#160;  X.template topLeftCorner&lt;3, 3&gt;() = R;</div><div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;  X.template bottomRightCorner&lt;3, 3&gt;() = R;</div><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;  X.template bottomLeftCorner&lt;3, 3&gt;() = -R * <a class="code" href="../../da/d9b/namespaceori.html#a153f2a76610f0c5524da3f8bf8fa4dfa">vectorToSkewMat</a>(r);</div><div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;  <span class="keywordflow">return</span> X;</div><div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;}</div><div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;</div><div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00157"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#abdc3cd991206414d80d9f89be27e3633">  157</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#abdc3cd991206414d80d9f89be27e3633">rotationFromSXform</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; X) {</div><div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;  static_assert(T::ColsAtCompileTime == 6 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;                <span class="stringliteral">&quot;Must have 6x6 matrix&quot;</span>);</div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;typename T::Scalar&gt;</a> R = X.template topLeftCorner&lt;3, 3&gt;();</div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;  <span class="keywordflow">return</span> R;</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;}</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;</div><div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00168"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#aba5e712bd3844d2e6a57dcb071f3f0a5">  168</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#aba5e712bd3844d2e6a57dcb071f3f0a5">translationFromSXform</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; X) {</div><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;  static_assert(T::ColsAtCompileTime == 6 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;                <span class="stringliteral">&quot;Must have 6x6 matrix&quot;</span>);</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;typename T::Scalar&gt;</a> R = <a class="code" href="../../de/d1a/namespacespatial.html#abdc3cd991206414d80d9f89be27e3633">rotationFromSXform</a>(X);</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> r =</div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;      -<a class="code" href="../../da/d9b/namespaceori.html#a0d9277fde185aecf3eae4a6d6450d25f">matToSkewVec</a>(R.transpose() * X.template bottomLeftCorner&lt;3, 3&gt;());</div><div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;  <span class="keywordflow">return</span> r;</div><div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;}</div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;</div><div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00181"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#af37757ee8ad4510df5478202a62c9502">  181</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#af37757ee8ad4510df5478202a62c9502">invertSXform</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; X) {</div><div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;  static_assert(T::ColsAtCompileTime == 6 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;                <span class="stringliteral">&quot;Must have 6x6 matrix&quot;</span>);</div><div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;typename T::Scalar&gt;</a> R = <a class="code" href="../../de/d1a/namespacespatial.html#abdc3cd991206414d80d9f89be27e3633">rotationFromSXform</a>(X);</div><div class="line"><a name="l00185"></a><span class="lineno">  185</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> r =</div><div class="line"><a name="l00186"></a><span class="lineno">  186</span>&#160;      -<a class="code" href="../../da/d9b/namespaceori.html#a0d9277fde185aecf3eae4a6d6450d25f">matToSkewVec</a>(R.transpose() * X.template bottomLeftCorner&lt;3, 3&gt;());</div><div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a217c4c7a22cd434b9700b183b07f1505">SXform&lt;typename T::Scalar&gt;</a> Xinv = <a class="code" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">createSXform</a>(R.transpose(), -R * r);</div><div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;  <span class="keywordflow">return</span> Xinv;</div><div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;}</div><div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;</div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00195"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#ae1f2e71c723cc6a785153b3c92a82e84">  195</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> <a class="code" href="../../de/d1a/namespacespatial.html#ae1f2e71c723cc6a785153b3c92a82e84">jointMotionSubspace</a>(<a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51">JointType</a> joint, <a class="code" href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5">CoordinateAxis</a> axis) {</div><div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> v(0, 0, 0);</div><div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;</a> phi = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;::Zero</a>();</div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;  <span class="keywordflow">if</span> (axis == CoordinateAxis::X)</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;    v(0) = 1;</div><div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;  <span class="keywordflow">else</span> <span class="keywordflow">if</span> (axis == CoordinateAxis::Y)</div><div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;    v(1) = 1;</div><div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;  <span class="keywordflow">else</span></div><div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;    v(2) = 1;</div><div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;</div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;  <span class="keywordflow">if</span> (joint == <a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51a35fa8acd1b8b1beb23e6c2e24313570c">JointType::Prismatic</a>)</div><div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;    phi.template bottomLeftCorner&lt;3, 1&gt;() = v;</div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;  <span class="keywordflow">else</span> <span class="keywordflow">if</span> (joint == <a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51a358561ac4d41f41d479b7c54b19b639e">JointType::Revolute</a>)</div><div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;    phi.template topLeftCorner&lt;3, 1&gt;() = v;</div><div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;  <span class="keywordflow">else</span></div><div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;    <span class="keywordflow">throw</span> std::runtime_error(<span class="stringliteral">&quot;Unknown motion subspace&quot;</span>);</div><div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;</div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;  <span class="keywordflow">return</span> phi;</div><div class="line"><a name="l00213"></a><span class="lineno">  213</span>&#160;}</div><div class="line"><a name="l00214"></a><span class="lineno">  214</span>&#160;</div><div class="line"><a name="l00218"></a><span class="lineno">  218</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00219"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#a6440807f3b1854b1708f49048331811a">  219</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> <a class="code" href="../../de/d1a/namespacespatial.html#a6440807f3b1854b1708f49048331811a">jointXform</a>(<a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51">JointType</a> joint, <a class="code" href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5">CoordinateAxis</a> axis, T q) {</div><div class="line"><a name="l00220"></a><span class="lineno">  220</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> <a class="code" href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383">X</a> = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;::Zero</a>();</div><div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;  <span class="keywordflow">if</span> (joint == <a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51a358561ac4d41f41d479b7c54b19b639e">JointType::Revolute</a>) {</div><div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;    X = <a class="code" href="../../de/d1a/namespacespatial.html#acab0e3d82f9a5385d482b83f2ee88eb1">spatialRotation</a>(axis, q);</div><div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;  } <span class="keywordflow">else</span> <span class="keywordflow">if</span> (joint == <a class="code" href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51a35fa8acd1b8b1beb23e6c2e24313570c">JointType::Prismatic</a>) {</div><div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> v(0, 0, 0);</div><div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;    <span class="keywordflow">if</span> (axis == CoordinateAxis::X)</div><div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;      v(0) = q;</div><div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (axis == CoordinateAxis::Y)</div><div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;      v(1) = q;</div><div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (axis == CoordinateAxis::Z)</div><div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;      v(2) = q;</div><div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;</div><div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;    X = <a class="code" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">createSXform</a>(<a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;T&gt;::Identity</a>(), v);</div><div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;  } <span class="keywordflow">else</span> {</div><div class="line"><a name="l00234"></a><span class="lineno">  234</span>&#160;    <span class="keywordflow">throw</span> std::runtime_error(<span class="stringliteral">&quot;Unknown joint xform\n&quot;</span>);</div><div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;  }</div><div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;  <span class="keywordflow">return</span> X;</div><div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;}</div><div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;</div><div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00245"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#aa7ac11b744e5fad0c1cbef41e3bb209f">  245</a></span>&#160;<a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;typename T::Scalar&gt;</a> <a class="code" href="../../de/d1a/namespacespatial.html#aa7ac11b744e5fad0c1cbef41e3bb209f">rotInertiaOfBox</a>(<span class="keyword">typename</span> T::Scalar mass,</div><div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;                                         <span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; dims) {</div><div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;  static_assert(T::ColsAtCompileTime == 1 &amp;&amp; T::RowsAtCompileTime == 3,</div><div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;                <span class="stringliteral">&quot;Must have 3x1 vector&quot;</span>);</div><div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;typename T::Scalar&gt;</a> I =</div><div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;      <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;typename T::Scalar&gt;::Identity</a>() * dims.norm() * dims.norm();</div><div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; 3; i++) I(i, i) -= dims(i) * dims(i);</div><div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;  I = I * mass / 12;</div><div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;  <span class="keywordflow">return</span> I;</div><div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;}</div><div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;</div><div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T, <span class="keyword">typename</span> T2&gt;</div><div class="line"><a name="l00261"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#a95b2db654a989f56df5a8977a5be12d6">  261</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#a95b2db654a989f56df5a8977a5be12d6">spatialToLinearVelocity</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; v,</div><div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;                             <span class="keyword">const</span> Eigen::MatrixBase&lt;T2&gt;&amp; x) {</div><div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;  static_assert(T::ColsAtCompileTime == 1 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;                <span class="stringliteral">&quot;Must have 6x1 vector&quot;</span>);</div><div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;  static_assert(T2::ColsAtCompileTime == 1 &amp;&amp; T2::RowsAtCompileTime == 3,</div><div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;                <span class="stringliteral">&quot;Must have 3x1 vector&quot;</span>);</div><div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> vsAng = v.template topLeftCorner&lt;3, 1&gt;();</div><div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> vsLin = v.template bottomLeftCorner&lt;3, 1&gt;();</div><div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> vLinear = vsLin + vsAng.cross(x);</div><div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;  <span class="keywordflow">return</span> vLinear;</div><div class="line"><a name="l00271"></a><span class="lineno">  271</span>&#160;}</div><div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;</div><div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00277"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#af4cad4343e60d07ebdb543fb9a121a67">  277</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#af4cad4343e60d07ebdb543fb9a121a67">spatialToAngularVelocity</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; v) {</div><div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;  static_assert(T::ColsAtCompileTime == 1 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;                <span class="stringliteral">&quot;Must have 6x1 vector&quot;</span>);</div><div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> vsAng = v.template topLeftCorner&lt;3, 1&gt;();</div><div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;  <span class="keywordflow">return</span> vsAng;</div><div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;}</div><div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;</div><div class="line"><a name="l00288"></a><span class="lineno">  288</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T, <span class="keyword">typename</span> T2&gt;</div><div class="line"><a name="l00289"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#a13df9ec1359ecd4150251eaae0895198">  289</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#a13df9ec1359ecd4150251eaae0895198">spatialToLinearAcceleration</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; a,</div><div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;                                 <span class="keyword">const</span> Eigen::MatrixBase&lt;T2&gt;&amp; v) {</div><div class="line"><a name="l00291"></a><span class="lineno">  291</span>&#160;  static_assert(T::ColsAtCompileTime == 1 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00292"></a><span class="lineno">  292</span>&#160;                <span class="stringliteral">&quot;Must have 6x1 vector&quot;</span>);</div><div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;  static_assert(T2::ColsAtCompileTime == 1 &amp;&amp; T2::RowsAtCompileTime == 6,</div><div class="line"><a name="l00294"></a><span class="lineno">  294</span>&#160;                <span class="stringliteral">&quot;Must have 6x1 vector&quot;</span>);</div><div class="line"><a name="l00295"></a><span class="lineno">  295</span>&#160;</div><div class="line"><a name="l00296"></a><span class="lineno">  296</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> acc;</div><div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;  <span class="comment">// classical accleration = spatial linear acc + omega x v</span></div><div class="line"><a name="l00298"></a><span class="lineno">  298</span>&#160;  acc = a.template tail&lt;3&gt;() + v.template head&lt;3&gt;().cross(v.template tail&lt;3&gt;());</div><div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;  <span class="keywordflow">return</span> acc;</div><div class="line"><a name="l00300"></a><span class="lineno">  300</span>&#160;}</div><div class="line"><a name="l00301"></a><span class="lineno">  301</span>&#160;</div><div class="line"><a name="l00306"></a><span class="lineno">  306</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T, <span class="keyword">typename</span> T2, <span class="keyword">typename</span> T3&gt;</div><div class="line"><a name="l00307"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#a791e1bf3407f16ee4157ff943ea824dc">  307</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#a13df9ec1359ecd4150251eaae0895198">spatialToLinearAcceleration</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; a,</div><div class="line"><a name="l00308"></a><span class="lineno">  308</span>&#160;                                 <span class="keyword">const</span> Eigen::MatrixBase&lt;T2&gt;&amp; v,</div><div class="line"><a name="l00309"></a><span class="lineno">  309</span>&#160;                                 <span class="keyword">const</span> Eigen::MatrixBase&lt;T3&gt;&amp; x) {</div><div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;  static_assert(T::ColsAtCompileTime == 1 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00311"></a><span class="lineno">  311</span>&#160;                <span class="stringliteral">&quot;Must have 6x1 vector&quot;</span>);</div><div class="line"><a name="l00312"></a><span class="lineno">  312</span>&#160;  static_assert(T2::ColsAtCompileTime == 1 &amp;&amp; T2::RowsAtCompileTime == 6,</div><div class="line"><a name="l00313"></a><span class="lineno">  313</span>&#160;                <span class="stringliteral">&quot;Must have 6x1 vector&quot;</span>);</div><div class="line"><a name="l00314"></a><span class="lineno">  314</span>&#160;  static_assert(T3::ColsAtCompileTime == 1 &amp;&amp; T3::RowsAtCompileTime == 3,</div><div class="line"><a name="l00315"></a><span class="lineno">  315</span>&#160;                <span class="stringliteral">&quot;Must have 3x1 vector&quot;</span>);</div><div class="line"><a name="l00316"></a><span class="lineno">  316</span>&#160;</div><div class="line"><a name="l00317"></a><span class="lineno">  317</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> alin_x = <a class="code" href="../../de/d1a/namespacespatial.html#a95b2db654a989f56df5a8977a5be12d6">spatialToLinearVelocity</a>(a, x);</div><div class="line"><a name="l00318"></a><span class="lineno">  318</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> vlin_x = <a class="code" href="../../de/d1a/namespacespatial.html#a95b2db654a989f56df5a8977a5be12d6">spatialToLinearVelocity</a>(v, x);</div><div class="line"><a name="l00319"></a><span class="lineno">  319</span>&#160;</div><div class="line"><a name="l00320"></a><span class="lineno">  320</span>&#160;  <span class="comment">// classical accleration = spatial linear acc + omega x v</span></div><div class="line"><a name="l00321"></a><span class="lineno">  321</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> acc = alin_x + v.template head&lt;3&gt;().cross(vlin_x);</div><div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;  <span class="keywordflow">return</span> acc;</div><div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;}</div><div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;</div><div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T, <span class="keyword">typename</span> T2&gt;</div><div class="line"><a name="l00329"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#ab521121fc7af2392144a78003c100a10">  329</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#ab521121fc7af2392144a78003c100a10">sXFormPoint</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; X,</div><div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;                 <span class="keyword">const</span> Eigen::MatrixBase&lt;T2&gt;&amp; p) {</div><div class="line"><a name="l00331"></a><span class="lineno">  331</span>&#160;  static_assert(T::ColsAtCompileTime == 6 &amp;&amp; T::RowsAtCompileTime == 6,</div><div class="line"><a name="l00332"></a><span class="lineno">  332</span>&#160;                <span class="stringliteral">&quot;Must have 6x6 vector&quot;</span>);</div><div class="line"><a name="l00333"></a><span class="lineno">  333</span>&#160;  static_assert(T2::ColsAtCompileTime == 1 &amp;&amp; T2::RowsAtCompileTime == 3,</div><div class="line"><a name="l00334"></a><span class="lineno">  334</span>&#160;                <span class="stringliteral">&quot;Must have 3x1 vector&quot;</span>);</div><div class="line"><a name="l00335"></a><span class="lineno">  335</span>&#160;</div><div class="line"><a name="l00336"></a><span class="lineno">  336</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;typename T::Scalar&gt;</a> R = <a class="code" href="../../de/d1a/namespacespatial.html#abdc3cd991206414d80d9f89be27e3633">rotationFromSXform</a>(X);</div><div class="line"><a name="l00337"></a><span class="lineno">  337</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> r = <a class="code" href="../../de/d1a/namespacespatial.html#aba5e712bd3844d2e6a57dcb071f3f0a5">translationFromSXform</a>(X);</div><div class="line"><a name="l00338"></a><span class="lineno">  338</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;typename T::Scalar&gt;</a> Xp = R * (p - r);</div><div class="line"><a name="l00339"></a><span class="lineno">  339</span>&#160;  <span class="keywordflow">return</span> Xp;</div><div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;}</div><div class="line"><a name="l00341"></a><span class="lineno">  341</span>&#160;</div><div class="line"><a name="l00347"></a><span class="lineno">  347</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T, <span class="keyword">typename</span> T2&gt;</div><div class="line"><a name="l00348"></a><span class="lineno"><a class="line" href="../../de/d1a/namespacespatial.html#a3b32f3f809891285d9f1a8cf3c7cd16e">  348</a></span>&#160;<span class="keyword">auto</span> <a class="code" href="../../de/d1a/namespacespatial.html#a3b32f3f809891285d9f1a8cf3c7cd16e">forceToSpatialForce</a>(<span class="keyword">const</span> Eigen::MatrixBase&lt;T&gt;&amp; <a class="code" href="../../dc/dd7/test__casadi_8cpp.html#ae20d7fba59865a21504b65cdb44cbfe5">f</a>,</div><div class="line"><a name="l00349"></a><span class="lineno">  349</span>&#160;                         <span class="keyword">const</span> Eigen::MatrixBase&lt;T2&gt;&amp; p) {</div><div class="line"><a name="l00350"></a><span class="lineno">  350</span>&#160;  static_assert(T::ColsAtCompileTime == 1 &amp;&amp; T::RowsAtCompileTime == 3,</div><div class="line"><a name="l00351"></a><span class="lineno">  351</span>&#160;                <span class="stringliteral">&quot;Must have 3x1 vector&quot;</span>);</div><div class="line"><a name="l00352"></a><span class="lineno">  352</span>&#160;  static_assert(T2::ColsAtCompileTime == 1 &amp;&amp; T2::RowsAtCompileTime == 3,</div><div class="line"><a name="l00353"></a><span class="lineno">  353</span>&#160;                <span class="stringliteral">&quot;Must have 3x1 vector&quot;</span>);</div><div class="line"><a name="l00354"></a><span class="lineno">  354</span>&#160;  <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;typename T::Scalar&gt;</a> fs;</div><div class="line"><a name="l00355"></a><span class="lineno">  355</span>&#160;  fs.template topLeftCorner&lt;3, 1&gt;() = p.cross(f);</div><div class="line"><a name="l00356"></a><span class="lineno">  356</span>&#160;  fs.template bottomLeftCorner&lt;3, 1&gt;() = f;</div><div class="line"><a name="l00357"></a><span class="lineno">  357</span>&#160;  <span class="keywordflow">return</span> fs;</div><div class="line"><a name="l00358"></a><span class="lineno">  358</span>&#160;}</div><div class="line"><a name="l00359"></a><span class="lineno">  359</span>&#160;</div><div class="line"><a name="l00360"></a><span class="lineno">  360</span>&#160;}  <span class="comment">// namespace spatial</span></div><div class="line"><a name="l00361"></a><span class="lineno">  361</span>&#160;</div><div class="line"><a name="l00362"></a><span class="lineno">  362</span>&#160;<span class="preprocessor">#endif  // LIBBIOMIMETICS_SPATIAL_H</span></div><div class="ttc" id="namespacespatial_html_a3b32f3f809891285d9f1a8cf3c7cd16e"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a3b32f3f809891285d9f1a8cf3c7cd16e">spatial::forceToSpatialForce</a></div><div class="ttdeci">auto forceToSpatialForce(const Eigen::MatrixBase&lt; T &gt; &amp;f, const Eigen::MatrixBase&lt; T2 &gt; &amp;p)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00348">spatial.h:348</a></div></div>
<div class="ttc" id="namespacespatial_html_a95b2db654a989f56df5a8977a5be12d6"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a95b2db654a989f56df5a8977a5be12d6">spatial::spatialToLinearVelocity</a></div><div class="ttdeci">auto spatialToLinearVelocity(const Eigen::MatrixBase&lt; T &gt; &amp;v, const Eigen::MatrixBase&lt; T2 &gt; &amp;x)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00261">spatial.h:261</a></div></div>
<div class="ttc" id="namespacespatial_html_a705e44e685fd39dbd6c1a9002ae9a344"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a705e44e685fd39dbd6c1a9002ae9a344">spatial::forceCrossProduct</a></div><div class="ttdeci">auto forceCrossProduct(const Eigen::MatrixBase&lt; T &gt; &amp;a, const Eigen::MatrixBase&lt; T &gt; &amp;b)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00091">spatial.h:91</a></div></div>
<div class="ttc" id="namespaceori_html_a537b8d0769c03e4cd8ef2b9a9dc8a760"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a537b8d0769c03e4cd8ef2b9a9dc8a760">ori::coordinateRotation</a></div><div class="ttdeci">Mat3&lt; T &gt; coordinateRotation(CoordinateAxis axis, T theta)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00060">orientation_tools.h:60</a></div></div>
<div class="ttc" id="cpp_types_8h_html_ae8ab6bd4079a5317297be5eeb331b1ca"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 6, 6 &gt; Mat6</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00070">cppTypes.h:70</a></div></div>
<div class="ttc" id="namespacespatial_html_a6440807f3b1854b1708f49048331811a"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a6440807f3b1854b1708f49048331811a">spatial::jointXform</a></div><div class="ttdeci">Mat6&lt; T &gt; jointXform(JointType joint, CoordinateAxis axis, T q)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00219">spatial.h:219</a></div></div>
<div class="ttc" id="cpp_types_8h_html_aa8cb424e643d476ea485238dad7c3e18"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, 3 &gt; Mat3</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00054">cppTypes.h:54</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a5e48cf69558cf96d92f71803cf9d19a4"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a5e48cf69558cf96d92f71803cf9d19a4">Mat4</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 4, 4 &gt; Mat4</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00094">cppTypes.h:94</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a6f07728c866b15ff0676a9b2ebfd2ba6"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, 1 &gt; Vec3</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00026">cppTypes.h:26</a></div></div>
<div class="ttc" id="namespacespatial_html_acc851583f244f19d9ea07490e9328b51a70739581ffec4f3db3f3fe5c0a309c3a"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51a70739581ffec4f3db3f3fe5c0a309c3a">spatial::JointType::FloatingBase</a></div></div>
<div class="ttc" id="namespacespatial_html_aba5e712bd3844d2e6a57dcb071f3f0a5"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#aba5e712bd3844d2e6a57dcb071f3f0a5">spatial::translationFromSXform</a></div><div class="ttdeci">auto translationFromSXform(const Eigen::MatrixBase&lt; T &gt; &amp;X)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00168">spatial.h:168</a></div></div>
<div class="ttc" id="namespacespatial_html_a13df9ec1359ecd4150251eaae0895198"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a13df9ec1359ecd4150251eaae0895198">spatial::spatialToLinearAcceleration</a></div><div class="ttdeci">auto spatialToLinearAcceleration(const Eigen::MatrixBase&lt; T &gt; &amp;a, const Eigen::MatrixBase&lt; T2 &gt; &amp;v)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00289">spatial.h:289</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a217c4c7a22cd434b9700b183b07f1505"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a217c4c7a22cd434b9700b183b07f1505">SXform</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 6, 6 &gt; SXform</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00066">cppTypes.h:66</a></div></div>
<div class="ttc" id="namespacespatial_html_ab521121fc7af2392144a78003c100a10"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#ab521121fc7af2392144a78003c100a10">spatial::sXFormPoint</a></div><div class="ttdeci">auto sXFormPoint(const Eigen::MatrixBase&lt; T &gt; &amp;X, const Eigen::MatrixBase&lt; T2 &gt; &amp;p)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00329">spatial.h:329</a></div></div>
<div class="ttc" id="namespaceori_html_a02c20a1b1d35988c657aa1e219d609d5"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5">ori::CoordinateAxis</a></div><div class="ttdeci">CoordinateAxis</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00032">orientation_tools.h:32</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a3d70f8eb9cf1855ca989f887a5543acc"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 6, 1 &gt; SVec</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00062">cppTypes.h:62</a></div></div>
<div class="ttc" id="namespacespatial_html_acab0e3d82f9a5385d482b83f2ee88eb1"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#acab0e3d82f9a5385d482b83f2ee88eb1">spatial::spatialRotation</a></div><div class="ttdeci">SXform&lt; T &gt; spatialRotation(CoordinateAxis axis, T theta)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00028">spatial.h:28</a></div></div>
<div class="ttc" id="namespacespatial_html_acd5568b97b640537ab22aaf458c73980"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#acd5568b97b640537ab22aaf458c73980">spatial::motionCrossMatrix</a></div><div class="ttdeci">auto motionCrossMatrix(const Eigen::MatrixBase&lt; T &gt; &amp;v)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00043">spatial.h:43</a></div></div>
<div class="ttc" id="namespacespatial_html_ae4a2145f3028087511b4f17c0152ec83"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">spatial::createSXform</a></div><div class="ttdeci">auto createSXform(const Eigen::MatrixBase&lt; T &gt; &amp;R, const Eigen::MatrixBase&lt; T2 &gt; &amp;r)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00140">spatial.h:140</a></div></div>
<div class="ttc" id="namespacespatial_html_a30b33586f68f1f6b31718848914a90ca"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a30b33586f68f1f6b31718848914a90ca">spatial::sxformToHomogeneous</a></div><div class="ttdeci">auto sxformToHomogeneous(const Eigen::MatrixBase&lt; T &gt; &amp;X)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00108">spatial.h:108</a></div></div>
<div class="ttc" id="namespacespatial_html_acc851583f244f19d9ea07490e9328b51a358561ac4d41f41d479b7c54b19b639e"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51a358561ac4d41f41d479b7c54b19b639e">spatial::JointType::Revolute</a></div></div>
<div class="ttc" id="namespacespatial_html_a49f73699d3c57d07835e8ab61af3f34a"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a49f73699d3c57d07835e8ab61af3f34a">spatial::motionCrossProduct</a></div><div class="ttdeci">auto motionCrossProduct(const Eigen::MatrixBase&lt; T &gt; &amp;a, const Eigen::MatrixBase&lt; T &gt; &amp;b)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00073">spatial.h:73</a></div></div>
<div class="ttc" id="namespacespatial_html_acc851583f244f19d9ea07490e9328b51"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51">spatial::JointType</a></div><div class="ttdeci">JointType</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00021">spatial.h:21</a></div></div>
<div class="ttc" id="namespacespatial_html_acc851583f244f19d9ea07490e9328b51a35fa8acd1b8b1beb23e6c2e24313570c"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51a35fa8acd1b8b1beb23e6c2e24313570c">spatial::JointType::Prismatic</a></div></div>
<div class="ttc" id="namespacespatial_html_a39cbc59fb7656b397d13c5a52799bf22"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a39cbc59fb7656b397d13c5a52799bf22">spatial::homogeneousToSXform</a></div><div class="ttdeci">auto homogeneousToSXform(const Eigen::MatrixBase&lt; T &gt; &amp;H)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00124">spatial.h:124</a></div></div>
<div class="ttc" id="namespacespatial_html"><div class="ttname"><a href="../../de/d1a/namespacespatial.html">spatial</a></div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00019">spatial.h:19</a></div></div>
<div class="ttc" id="namespaceori_html_a153f2a76610f0c5524da3f8bf8fa4dfa"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a153f2a76610f0c5524da3f8bf8fa4dfa">ori::vectorToSkewMat</a></div><div class="ttdeci">Mat3&lt; typename T::Scalar &gt; vectorToSkewMat(const Eigen::MatrixBase&lt; T &gt; &amp;v)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00096">orientation_tools.h:96</a></div></div>
<div class="ttc" id="orientation__tools_8h_html"><div class="ttname"><a href="../../d4/dfb/orientation__tools_8h.html">orientation_tools.h</a></div><div class="ttdoc">Utility functions for 3D rotations. </div></div>
<div class="ttc" id="namespaceori_html_a0d9277fde185aecf3eae4a6d6450d25f"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a0d9277fde185aecf3eae4a6d6450d25f">ori::matToSkewVec</a></div><div class="ttdeci">Vec3&lt; typename T::Scalar &gt; matToSkewVec(const Eigen::MatrixBase&lt; T &gt; &amp;m)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00108">orientation_tools.h:108</a></div></div>
<div class="ttc" id="namespaceori_html"><div class="ttname"><a href="../../da/d9b/namespaceori.html">ori</a></div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00028">orientation_tools.h:28</a></div></div>
<div class="ttc" id="namespacespatial_html_ae1f2e71c723cc6a785153b3c92a82e84"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#ae1f2e71c723cc6a785153b3c92a82e84">spatial::jointMotionSubspace</a></div><div class="ttdeci">SVec&lt; T &gt; jointMotionSubspace(JointType joint, CoordinateAxis axis)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00195">spatial.h:195</a></div></div>
<div class="ttc" id="namespaceori_html_a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383">ori::CoordinateAxis::X</a></div></div>
<div class="ttc" id="namespacespatial_html_a261bcef5a53b2fb64a5c7370420a1275"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a261bcef5a53b2fb64a5c7370420a1275">spatial::forceCrossMatrix</a></div><div class="ttdeci">auto forceCrossMatrix(const Eigen::MatrixBase&lt; T &gt; &amp;v)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00060">spatial.h:60</a></div></div>
<div class="ttc" id="namespacespatial_html_aa7ac11b744e5fad0c1cbef41e3bb209f"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#aa7ac11b744e5fad0c1cbef41e3bb209f">spatial::rotInertiaOfBox</a></div><div class="ttdeci">Mat3&lt; typename T::Scalar &gt; rotInertiaOfBox(typename T::Scalar mass, const Eigen::MatrixBase&lt; T &gt; &amp;dims)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00245">spatial.h:245</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a0b83d93a3c74d282558436e26c056896"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, 3 &gt; RotMat</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00018">cppTypes.h:18</a></div></div>
<div class="ttc" id="namespacespatial_html_abdc3cd991206414d80d9f89be27e3633"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#abdc3cd991206414d80d9f89be27e3633">spatial::rotationFromSXform</a></div><div class="ttdeci">auto rotationFromSXform(const Eigen::MatrixBase&lt; T &gt; &amp;X)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00157">spatial.h:157</a></div></div>
<div class="ttc" id="namespacespatial_html_acc851583f244f19d9ea07490e9328b51af80a4ad87fee7c9fdc19b7769495fdb5"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#acc851583f244f19d9ea07490e9328b51af80a4ad87fee7c9fdc19b7769495fdb5">spatial::JointType::Nothing</a></div></div>
<div class="ttc" id="namespacespatial_html_af4cad4343e60d07ebdb543fb9a121a67"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#af4cad4343e60d07ebdb543fb9a121a67">spatial::spatialToAngularVelocity</a></div><div class="ttdeci">auto spatialToAngularVelocity(const Eigen::MatrixBase&lt; T &gt; &amp;v)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00277">spatial.h:277</a></div></div>
<div class="ttc" id="test__casadi_8cpp_html_ae20d7fba59865a21504b65cdb44cbfe5"><div class="ttname"><a href="../../dc/dd7/test__casadi_8cpp.html#ae20d7fba59865a21504b65cdb44cbfe5">f</a></div><div class="ttdeci">MX f(const MX &amp;x, const MX &amp;u)</div><div class="ttdef"><b>Definition:</b> <a href="../../dc/dd7/test__casadi_8cpp_source.html#l00202">test_casadi.cpp:202</a></div></div>
<div class="ttc" id="namespacespatial_html_af37757ee8ad4510df5478202a62c9502"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#af37757ee8ad4510df5478202a62c9502">spatial::invertSXform</a></div><div class="ttdeci">auto invertSXform(const Eigen::MatrixBase&lt; T &gt; &amp;X)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00181">spatial.h:181</a></div></div>
</div><!-- fragment --></div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="navelem"><a class="el" href="../../dir_43e0a1f539e00dcfa1a6bc4d4fee4fc2.html">home</a></li><li class="navelem"><a class="el" href="../../dir_61bfb0b3cdda81b1b0355ba34eb3b1eb.html">dhkim</a></li><li class="navelem"><a class="el" href="../../dir_adee5b101e079090017bb4e91c628ead.html">Public_Repository</a></li><li class="navelem"><a class="el" href="../../dir_7fc07a7b97001e7b1d18e11da761e2de.html">Cheetah-Software</a></li><li class="navelem"><a class="el" href="../../dir_860379c26524a894b3cf97b5b1ba5bdb.html">common</a></li><li class="navelem"><a class="el" href="../../dir_275ce76ae8a9e2090fbb7142d3994a0e.html">include</a></li><li class="navelem"><a class="el" href="../../dir_5f249a132d763dc59b0ff1086b98b7c1.html">Dynamics</a></li><li class="navelem"><a class="el" href="../../d9/dc8/spatial_8h.html">spatial.h</a></li>
    <li class="footer">Generated by
    <a href="http://www.doxygen.org/index.html">
    <img class="footer" src="../../doxygen.png" alt="doxygen"/></a> 1.8.11 </li>
  </ul>
</div>
</body>
</html>
